// Matrix.cpp: implementation of the CMatrix class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "Matrix.h"
#include "math.h"
#include "Transform.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
double CMatrix::Pi=3.1415926;

CMatrix::CMatrix()
{
	m_Data[0][0]=m_Data[0][1]=m_Data[0][2]=0;
	m_Data[1][0]=m_Data[1][1]=m_Data[1][2]=0;
	m_Data[2][0]=m_Data[2][1]=m_Data[2][2]=0;
}

CMatrix::~CMatrix()
{

}

//CMatrix CMatrix::DotProduct(const CMatrix & mat) const
//{
//	CMatrix m;
//	return m;
//}

CMatrix CMatrix::CrossProduct(const CMatrix & mat) const
{
	CMatrix m;
	m.SetValue(0, 0, m_Data[0][0]*mat.GetValue(0, 0)+m_Data[0][1]*mat.GetValue(1, 0)+m_Data[0][2]*mat.GetValue(2, 0));
	m.SetValue(0, 1, m_Data[0][0]*mat.GetValue(0, 1)+m_Data[0][1]*mat.GetValue(1, 1)+m_Data[0][2]*mat.GetValue(2, 1));
	m.SetValue(0, 2, m_Data[0][0]*mat.GetValue(0, 2)+m_Data[0][1]*mat.GetValue(1, 2)+m_Data[0][2]*mat.GetValue(2, 2));
	m.SetValue(1, 0, m_Data[1][0]*mat.GetValue(0, 0)+m_Data[1][1]*mat.GetValue(1, 0)+m_Data[1][2]*mat.GetValue(2, 0));
	m.SetValue(1, 1, m_Data[1][0]*mat.GetValue(0, 1)+m_Data[1][1]*mat.GetValue(1, 1)+m_Data[1][2]*mat.GetValue(2, 1));
	m.SetValue(1, 2, m_Data[1][0]*mat.GetValue(0, 2)+m_Data[1][1]*mat.GetValue(1, 2)+m_Data[1][2]*mat.GetValue(2, 2));
	m.SetValue(2, 0, m_Data[2][0]*mat.GetValue(0, 0)+m_Data[2][1]*mat.GetValue(1, 0)+m_Data[2][2]*mat.GetValue(2, 0));
	m.SetValue(2, 1, m_Data[2][0]*mat.GetValue(0, 1)+m_Data[2][1]*mat.GetValue(1, 1)+m_Data[2][2]*mat.GetValue(2, 1));
	m.SetValue(2, 2, m_Data[2][0]*mat.GetValue(0, 2)+m_Data[2][1]*mat.GetValue(1, 2)+m_Data[2][2]*mat.GetValue(2, 2));
	return m;
}

void CMatrix::TransformPoint(double x, double y, double & new_x, double & new_y) const
{
	new_x=m_Data[0][0]*x+m_Data[0][1]*y+m_Data[0][2];
	new_y=m_Data[1][0]*x+m_Data[1][1]*y+m_Data[1][2];
}

void CMatrix::TransformVector(double x, double y, double & new_x, double & new_y) const
{
	double x1, y1, x2, y2;
	TransformPoint(0, 0, x1, y1);
	TransformPoint(x, y, x2, y2);

	new_x=x2-x1;
	new_y=y2-y1;
}

void CMatrix::TransformAngle(double angle, double & new_angle)
{
	double x=cos(angle);
	double y=sin(angle);

	double new_x, new_y;
	TransformVector(x, y, new_x, new_y);
	//double len=sqrt(new_x*new_x+new_y*new_y);
	//new_angle=acos(new_x/len);
	//if(new_y<0)
	//	new_angle=2*CMathUtil::Pi-new_angle;
	new_angle=Angle(new_x, new_y);
}

void CMatrix::TransformLength(double dblWidth, double & dblNewWidth) const
{
	double x, y;
	TransformVector(dblWidth, 0, x, y);

	dblNewWidth=sqrt(x*x+y*y);
}

void CMatrix::TransformHeight(double dblHeight, double & dblNewHeight) const
{
	double x, y;
	TransformVector(0, dblHeight, x, y);

	dblNewHeight=sqrt(x*x+y*y);
}

bool CMatrix::SetValue(int i, int j, double dblValue)
{
	if(i<3 && j<3)
	{
		m_Data[i][j]=dblValue;
		return true;
	}
	return false;
}

bool CMatrix::GetValue(int i, int j, double & dblValue) const
{
	if(i<3 && j<3)
	{
		dblValue=m_Data[i][j];
		return true;
	}

	return false;
}

double CMatrix::GetValue(int i, int j) const
{
	if(i<3 && j<3)
		return m_Data[i][j];

	return 0;
}

void CMatrix::Get3Dtransf(float* float_mat)
{
	float_mat[0] = (float)m_Data[0][0];
	float_mat[1] = (float)m_Data[1][0];
	float_mat[2] = 0.0;
	float_mat[3] = (float)m_Data[2][0];

	float_mat[4] = (float)m_Data[0][1];
	float_mat[5] = (float)m_Data[1][1];
	float_mat[6] = 0.0;
	float_mat[7] = (float)m_Data[2][1];

	float_mat[8] = 0.0;
	float_mat[9] = 0.0;
	float_mat[10] = 1.0;
	float_mat[11] = 0.0;

	float_mat[12] = (float)m_Data[0][2];
	float_mat[13] = (float)m_Data[1][2];
	float_mat[14] = 0.0;
	float_mat[15] = (float)m_Data[2][2];
}

CMatrix CMatrix::Identity()
{
	CMatrix m;
	m.SetValue(0, 0, 1);
	m.SetValue(1, 1, 1);
	m.SetValue(2, 2, 1);

	return m;
}

CMatrix CMatrix::Translation(double dblOffsetX, double dblOffsetY)
{
	CMatrix m=CMatrix::Identity();
	m.SetValue(0, 2, dblOffsetX);
	m.SetValue(1, 2, dblOffsetY);

	return m;
}

CMatrix CMatrix::Rotation(double dblOriginX, double dblOriginY, double dblAngle)
{
	CMatrix m=CMatrix::Identity();

	double dblSinAngle=sin(dblAngle);
	double dblCosAngle=cos(dblAngle);
	m.SetValue(0, 0, dblCosAngle);
	m.SetValue(0, 1, -dblSinAngle);
	m.SetValue(0, 2, dblOriginX-dblOriginX*dblCosAngle+dblOriginY*dblSinAngle);
	m.SetValue(1, 0, dblSinAngle);
	m.SetValue(1, 1, dblCosAngle);
	m.SetValue(1, 2, dblOriginY-dblOriginX*dblSinAngle-dblOriginY*dblCosAngle);

	return m;
}

CMatrix CMatrix::Scale(double dblBaseX, double dblBaseY, double dblXScale, double dblYScale)
{
	CMatrix m1=CMatrix::Translation(-dblBaseX, -dblBaseY);
	CMatrix m2=CMatrix::Translation(dblBaseX, dblBaseY);

	CMatrix m=CMatrix::Identity();
	m.SetValue(0, 0, dblXScale);
	m.SetValue(1, 1, dblYScale);

	m=m.CrossProduct(m1);
	m=m2.CrossProduct(m);

	return m;
}

CMatrix CMatrix::Mirror(double dblOriginX, double dblOriginY, double dblDirX, double dblDirY)
{
	double dblAngleToYAxis = AngleBetween(dblDirX, dblDirY, 0, 1);
	double dblCos2a=cos(2*dblAngleToYAxis);
	double dblSin2a=sin(2*dblAngleToYAxis);

	CMatrix m=CMatrix::Identity();
	m.SetValue(0, 0, -dblCos2a);
	m.SetValue(0, 1, dblSin2a);
	m.SetValue(0, 2, dblOriginX*dblCos2a-dblOriginY*dblSin2a+dblOriginX);
	m.SetValue(1, 0, dblSin2a);
	m.SetValue(1, 1, dblCos2a);
	m.SetValue(1, 2, -dblOriginX*dblSin2a-dblOriginY*dblCos2a+dblOriginY);

	return m;
}

CMatrix CMatrix::Transf3Dto2D(float* float_mat)
{
	CMatrix m=CMatrix::Identity();

	m.SetValue(0, 0, float_mat[0]);
	m.SetValue(0, 1, float_mat[4]);
	m.SetValue(0, 2, float_mat[12]);

	m.SetValue(1, 0, float_mat[1]);
	m.SetValue(1, 1, float_mat[5]);
	m.SetValue(1, 2, float_mat[13]);

	m.SetValue(2, 0, float_mat[3]);
	m.SetValue(2, 1, float_mat[7]);
	m.SetValue(2, 2, float_mat[15]);

	return m;
}

CMatrix CMatrix::Inverse(CMatrix & mat)
{
	float float_OwnerMatrix[16];
	mat.Get3Dtransf(float_OwnerMatrix);
	float float_OwnerInverse[16];
	CMatrix::GetInverse(float_OwnerMatrix, float_OwnerInverse);
	CMatrix matrixInverse = CMatrix::Transf3Dto2D(float_OwnerInverse);

	return matrixInverse;
}

double CMatrix::AngleBetween(double x1, double y1, double x2, double y2)
{
	double r1=sqrt(x1*x1+y1*y1);
	double sin_alpha1=-y1/r1;
	double cos_alhpa1=x1/r1;
	double x=x2*cos_alhpa1-y2*sin_alpha1;
	double y=x2*sin_alpha1+y2*cos_alhpa1;

	double r=sqrt(x*x+y*y);
	double sin_alpha=y/r;
	double cos_alhpa=x/r;
	double alpha=acos(cos_alhpa);//0--180
	if(sin_alpha<0)//>180
		alpha=Pi*2-alpha;
	return alpha;
}

void CMatrix::GetInverse(float* float_mat, float* float_Inv)
{
	CTransform transf(float_mat[0], float_mat[1], float_mat[2], float_mat[3],
					  float_mat[4], float_mat[5], float_mat[6], float_mat[7],
					  float_mat[8], float_mat[9], float_mat[10], float_mat[11],
					  float_mat[12], float_mat[13], float_mat[14], float_mat[15]);

	CTransform transf_inverse = CTransform::inverse(transf);
	transf_inverse.Get(float_Inv);
}

double CMatrix::Angle(double x, double y)
{
	double r=sqrt(x*x+y*y);
	double sin_alpha=y/r;
	double cos_alhpa=x/r;
	double alpha=acos(cos_alhpa);//0--180

	if(sin_alpha<0)//>180
		alpha=Pi*2-alpha;
	return alpha;
}

bool CMatrix::IsSameMatrix( CMatrix A)
{
	bool bIsSame=true;
	for (int i=0;i<3;i++)
	{
		for (int j=0;j<3;j++)
		{
			if (A.m_Data[i][j]!=m_Data[i][j])
			{
				bIsSame=false;
			}
		}
	}	
	return bIsSame;
}
